package es.uji.viselab.math;

import es.uji.viselab.robot.links.fanuc.RobotPosition;

public class RobotToolLocation {

	protected PointXYZ location;
	protected PointXYZ orientation;

	public RobotToolLocation(int x, int y, int z, int w, int p, int r) {
		location = new PointXYZ(x, y, z);
		orientation = new PointXYZ(w, p, r);
	}

	public RobotToolLocation() {
		location = new PointXYZ();
		orientation = new PointXYZ();
	}

	public RobotToolLocation(PointXYZ locationxyz, PointXYZ orientationxyz) {
		location = locationxyz;
		orientation = orientationxyz;
	}

	public RobotToolLocation(RobotToolLocation rtl) {
		location = new PointXYZ(rtl.location);
		orientation = new PointXYZ(rtl.orientation);
	}

	public RobotToolLocation(RobotPosition pos) {
		location = new PointXYZ();
		location.setX(pos.getX());
		location.setY(pos.getY());
		location.setZ(pos.getZ());
		orientation = new PointXYZ();
		orientation.setX(pos.getW());
		orientation.setY(pos.getP());
		orientation.setZ(pos.getR());
	}

	public RobotToolLocation(double x, double y, double z, double w, double p, double r) {
		location = new PointXYZ(x, y, z);
		orientation = new PointXYZ(w, p, r);
	}

	public PointXYZ getLocation() {
		return location;
	}

	public void setLocation(PointXYZ location) {
		this.location = location;
	}

	public PointXYZ getOrientation() {
		return orientation;
	}

	public void setOrientation(PointXYZ orientation) {
		this.orientation = orientation;
	}

	public void increaseLocation(double d, double e, double f) {
        location.setX(location.getX()+d);
        location.setY(location.getY()+e);
        location.setZ(location.getZ()+f);
	}

	public void increaseLocation(float dx, float dy, float dz) {
        increaseLocation((int)dx,(int)dy,(int)dz);
	}

	public double[] getArray() {
        double e[] = new double[6];
		e[0] = getLocation().getX();
		e[1] = getLocation().getY();
		e[2] = getLocation().getZ();
		e[3] = getOrientation().getX();
		e[4] = getOrientation().getY();
		e[5] = getOrientation().getZ();
		return e;
	}

	public boolean equal(RobotToolLocation original) {
		return this.location.equal(original.location) && this.orientation.equal(original.orientation) ;
	}


}
